# Modulos locales
from log import *
from xmlParser import *

import pygame
import sys, os
import random
import math
import numpy
from pygame.locals import *

from globalVar import GLOBAL_VAR, agentes, LAST_STATE, VISILIBITY, LOGS


def separacion(agent, nearest):
    # Orientacion necesaria para 
    dx = nearest.rect.centerx - agent.rect.centerx 
    dy = nearest.rect.centery - agent.rect.centery
    theta_nearest = math.atan2(dy, dx)
    
    nearest_heading = math.degrees(theta_nearest)
    if ( nearest_heading < 0):
        nearest_heading += 360
    
    new_heading = min(nearest_heading+180,nearest_heading-180)
    return new_heading - agent.heading


def cohesion(agent, x_positions, y_positions, x_velocities, y_velocities, mases):
    '''Devuelve los grados que tiene que girar el agente para apuntar al centro
    de masa de sus vecinos. Si el resultado es negativo el agente tiene que girar
    esa cantidad de grados a izquiera, si el resultado es positivo los debe girar
    hacia la derecha'''
    
    # Calculo el centro de masa
    # El centro de masa es sum(m*x)/(sum m)
#    cm_x = sum(map(lambda x: x[0]*x[1], zip(x_positions, mases)))/len(mases)
#    cm_y = sum(map(lambda x: x[0]*x[1], zip(y_positions, mases)))/len(mases)

    cm_x = sum(map(lambda x: x[0]*x[1], zip(x_positions, mases)))/sum(mases)
    cm_y = sum(map(lambda x: x[0]*x[1], zip(y_positions, mases)))/sum(mases)
    
    # El angulo desde mi posicion hasta el centro de masa
    theta_media = math.atan2(cm_y - agent.rect.centery, cm_x - agent.rect.centerx)
    new_heading = math.degrees(theta_media)
    if ( new_heading < 0):
        new_heading += 360
    
    vel_media = sum([math.sqrt(v[0]**2 + v[1]**2) for v in zip(x_velocities, y_velocities)])/len(mases)
    
    return new_heading - agent.heading, vel_media

def alineacion(agent, x_velocities, y_velocities):
    '''Devuelve los grados que tiene que girar el agente para que su heading
    apunte a la media del heading de sus vecinos. Si el resultado es negativo 
    el agente tiene que girar esa cantidad de grados a izquiera, si el resultado
     es positivo los debe girar hacia la derecha'''
    
    aux_vx = sum(x_velocities)
    aux_vy = sum(y_velocities)
    
    # Calculo el angulo(en rad) de la suma de las velocidades
    theta_vel = math.atan2(aux_vy, aux_vx)
    new_heading = math.degrees(theta_vel)
    if ( new_heading < 0):
        new_heading += 360
        
    return new_heading - agent.heading

def sign(number):
    '''Devuelve el signo del numero si distinto de 0, y 0 caso contrario'''
    try:return number/abs(number)
    except ZeroDivisionError:return 0
# Desc: Funcion que administra los eventos sucedidos en la simulacion
# Ret: Nada
def input(events):
    global LOGS
    
    # Inicio bucle para cada evento
    for event in events:
        
        # Si el evento es de salida
        if event.type == QUIT:
            
            # Cerrar archivos de logs
            for log in LOGS:
                
                # Cierro archivos
                LOGS[log].file.close()
            
            # Salgo
            sys.exit(0)

        # Si se apreto una tecla
        elif event.type == pygame.KEYDOWN:

            # y esa tecla es ESCAPE
            if event.key == K_ESCAPE:

                # Guardo configuracion y estado actual
                save_current_state()
                
                #Cierro archivos de log
                close_logs()
                
                # Y salgo
                sys.exit(0)


def close_logs():
    
    # Cerrar archivos de logs
    for log in LOGS:
                
        # Cierro archivos
        LOGS[log].file.close()
                    
        if not LOGS[log].file.closed :
            print 'Error: Imposible cerrar archivo ',LOG['log'].name



# Desc: Funcion que guarda la configuracion y estado actual en un archivo .xml
# Ret: Nada
def save_current_state ():
    global GLOBAL_VAR, agentes, LAST_STATE
    
    # Obtengo y guardo la fecha actual
    hoy = datetime.now()
    
    # Recupero nombre de archivo de log
    log_path = LOGS['log'].name.split('/')
    
    # Genero directorio donde se guardara el estado final.
    # Este estara ubicado en un directorio dentro del directorio donde
    # se encuentra el archivo de log
    path = '/'.join(log_path[:-1])+'/'+LAST_STATE
    
    
    # Veo si existe el path log_path
    if not os.access(path, os.F_OK):
            # Creo el directorio pues no existe
            os.mkdir(path)
    
    # Abro archivo original de configuracion
    original = open (GLOBAL_VAR['CONFIG_FILE'], 'r')
    
    # Recupero el nombre de archivo del log
    log_file_name = log_path[-1]
    
    # Genero archivo .xml donde se guardara la configuracion y estado actual
    last_state = Log(path+'/'+'last_state_'+ log_file_name +'.xml')
    
    # Para cada linea del archivo de configuracion original
    for line in original.readlines():
        # Si la linea corresponde a la fecha
        if line.find('date') != -1 :
            # Recupero parte de la linea sin la fecha
            new_string = line[0:line.find('date')+6]
            
            # Genero nueva fecha, uso la actual
            new_date = str(hoy.day)+'/'+str(hoy.month)+'/'+str(hoy.year)
            
            # Termino de actualizar la nueva linea
            new_string += new_date+'\">\n'
            
            # Escribo nueva linea en el archivo nuevo
            last_state.update(new_string)
            
            # Paso a la siguiente linea del original
            continue
        
        # Si la linea corresponde al flag que dice si la posicion es random
        if line.find('random_position') != -1:
            # Recupero parte esencial de la linea sin el flag
            new_string = line[0:line.find('>')+1]
            
            # Seteo flag a FALSE
            new_string += 'False'+line[line.find('</'):]
            
            # Escribo nueva linea en el archivo nuevo
            last_state.update(new_string)
            
            # Paso a la siguiente linea del original
            continue
        
        # Si la linea corresponde a los datos iniciales de los agentes
        if line.find('data name') != -1:
            # Salgo
            break
        
        # Si es cualquier otra linea la copio identica
        last_state.update(line)
    
    # Aqui se cargaran los datos correspondientes a los agentes en el
    # estado que se desea salvar.
    for i in range(len(agentes)):
        # Genero nueva linea con datos de posicion y velocidad
        new_line ='    <data name=\"'
        new_line += str(i)+'\">\n'
        new_line +='      <x_pos>'+str(agentes[i].pos[0])+'</x_pos>\n'
        new_line +='      <y_pos>'+str(agentes[i].pos[1])+'</y_pos>\n'
        new_line +='      <x_vel>'+str(agentes[i].speed[0])+'</x_vel>\n'
        new_line +='      <y_vel>'+str(agentes[i].speed[1])+'</y_vel>\n'
        new_line +='    </data>\n'
        
        # Escribo nueva linea en el archivo nuevo
        last_state.update(new_line)
        
    # Escribo las ultimas lineas del archivo de configuracion nuevo
    last_state.update('  </agentes>\n')
    last_state.update('</configuration_file>')
    
# Desc: Funcion que carga la imagen 'path_to_image' usando pygame
# Ret: Imagen cargada y rectangulo de la misma
def load_image(path_to_image, keycolor=None):
    # Intento cargar imagen
    try:
        image = pygame.image.load(path_to_image)
    
    # Si hay error, imprimo mensaje y salgo.
    except pygame.error, message:
        print 'Imposible abrir '+path_to_image
        sys.exit(0)
    
    # Cambio el formato de pixeles al de mi screen
    image = image.convert()
    
    if keycolor is not None:
        if keycolor is -1:
            # Tomo el color del vertice sup izquiero
            keycolor = image.get_at((0,1))
        
        # Hago transparente ese color
        image.set_colorkey(keycolor, RLEACCEL)
    
    # Devuelvo la imagen y su rectangulo
    return image, image.get_rect()

# Desc: Funcion que toma un N y dos coordenadas 'center_x' y 'center_y'
#       y genera posiciones al azar en un circulo de radio R=N*l_a 
#       y centro en ('center_x', 'center_y')
# Ret: Coordenadas de la posicion al azar generada
def rand_position_on_circle(N, center_x=0, center_y=0):
    global GLOBAL_VAR
    
    # Defino el radio del circulo
    R = N*GLOBAL_VAR['l_a']
    
    # Genero distancia al azar del centro del circulo
    r = R*random.random()
    
    # Genero un angulo al azar
    theta = 2*math.pi*random.random()
    
    # Calculo las coordenadas del punto
    x = r*math.cos(theta)
    y = r*math.sin(theta)
    
    # Devuelvo coordenadas del punto generado
    return [x + center_x, y + center_y]

# Desc: Funcion que toma un N y dos coordenadas 'center_x' y 'center_y'
#       y genera posiciones al azar en un cuadrado de lado L=N*l_a 
#       y centro en ('center_x', 'center_y')
# Ret: Coordenadas de la posicion al azar generada
def rand_position_on_square(N, center_x=0, center_y=0):
    global GLOBAL_VAR
    
    # Defino la longitud de los lados
    L = N*l_a
    
    # Calculo las coordenadas del punto
    x = L*random.random()
    y = L*random.random()
    
    # Devuelvo coordenadas del punto generado
    return [x + center_x, y + center_y]

# Desc: Funcion que genera una velocidad random en el rango (0,MAX_VEL)
#       y con una direccion random tambien
# Ret: Velocidad generada por coordenadas
def rand_velocity(MAX_VEL):
    # Genero un angulo al azar
    angulo = 2*math.pi *random.random()
    
    # Genero modulo velocidad random en el rango (0,MAX_VEL)
    vel = random.randint(0,int(MAX_VEL*100)) / 100.0
    
    # Calculo y devuelvo la velocidad por coordenadas 
    return [vel*math.cos(angulo), vel*math.sin(angulo)]

# Desc: Funcion que calcula a que REGION de la Fig.1 del Paper 
#       corresponde la simulacion con los parametros que se pasan
# Ret: Un integer que identifica la region, entre 1 y 7
def get_region(Ca, Cr, la, lr):
    # Defino relacion Ca y Cr
    C = Cr / Ca
    
    # Defino relacion la y lr
    L = lr / la
    
    if max(C,L) <= 1:
        # estoy en el cuadrado con vertices en (0,0) y (1,1) 
        # donde se encuentran las regiones I, II y II
        
        if L < C:
            # Estoy en la region I
            region = 1
        
        elif L==C:
            # Estoy en la recta correspondiente a la region II
            region = 2

        elif L > C:
            # Estoy en la region III
            region = 3

    elif min(C,L) < 1:

        # estoy en las regiones IV, VI o VII
        if L > 1:
            #estoy en la region IV
            region = 4

        elif C*(L**2) > 1:
            # H-Stable
            #estoy en la region VI
            region = 6
        else:
            region = 7
    else:
        # H-Stable, region V
        region = 5
    
    # Devuelvo el numero de la region
    return region

# Desc: Despliega un menu donde enumera los archivos .xml que encuentra 
#       en el directorio 'path'.
# Ret: En caso de exito devuelve el nombre de archivo .xml elegido por 
#       el usuario. Si hubo error devuelve -1
def menu_choose_config(path='.'):
    # Lista que contedra los archivos .xml que se encuentren
    xml = []
    
    # Recorro todos los archivos
    for file in os.listdir(path):
        if file.endswith('.xml'):
            # Guardo los que terminan con .xml
            xml.append(file)
    
    # Si hay algun archivo .xml en el directorio 'path'
    if len(xml)>0:
        # Ordeno la lista 
        xml.sort()
        
        # Imprimo titulo del menu
        print '\n\t'+'*'*10+' MENU - SELECCIONE ARCHIVO DE CONFIGURACION '+'*'*10+'\n'
        
        # Guardo opcion elegida por el usuario
        opt = print_menu_number(xml)
        
        # Guardo nombre de archivo a devolver
        ret = xml[opt]
    else:
        # Si no hay archivos .xml imprimo mensaje de error
        print '\nError: No se encontraron archivos de configuracion.'
        
        # Devuelvo -1 porque hubo un error
        ret = -1
    
    # Devuelvo lo que corresponde
    return ret

# Desc: Funcion que despliega la lista 'list' en opciones enumeradas y pide
#       al usuario elegir una de ellas
# Ret: (Int)Numero de opcion elegida por el usuario
def print_menu_number(list):
    # Flag que indica si se debe terminar bucle que imprime opciones
    end = False
    
    while not end:
        # Variable asociado a opcion
        i = 0
        
        # Para cada elemento de la lista
        for l in list:
            
            # Imprimo un lebel de la forma ' i) option '
            print '\t\t'+str(i) +') '+l
            
            # Aumento numero de opcion
            i += 1        
        
        # Pido al usuario que ingrese un numero y lo guardo en variable 'opt'
        opt = int(raw_input('\n\tOPCION: '))
        
        # Si 'opt' es valido
        if 0 <= opt <= i:
            # Termino bucle de while
            end = True
            
        else:   # Si 'opt' es invalido

            # Imprimo mensaje de error y ejecuto otro ciclo de while
            print 'ERROR: Opcion no valida. Elija un numero entre 0 y '+str(i)
    
    # Devuelvo la opcion que el usuario eligio
    return opt

# Desc: Calcula la energia total del sistema
# Ret: Energia total del sistema
def total_kinetic():
    global agentes
    # Variable para guardar la energia cinetica total del sistema
    k = 0
    for agent in agentes:
        # Calculo la velocidad del agente
        vel  = math.sqrt(agent.speed[0]**2+ agent.speed[1]**2)
        
        # Sumo su energia cinetica a la total(k)
        k += 0.5*agent.masa *(vel**2) 
    # Devuelvo la energia total del sistema
    return k

def get_media_radio(agentes):

    # Obtengo coordenadas del centro de masa
    r_cm = centro_de_masa(agentes)

    # Varaible para guardar sumatoria de los radios de los 
    # agentes en funcion del centro de masa del sistema
    R = 0
    
    for agente in agentes:
        R += math.hypot( agente.pos[0] - r_cm[0], agente.pos[1] - r_cm[1] )
    
    # Calculo la media del radio
    media_radio = R / len(agentes)
    
    # Devuelvo la media del radio
    return media_radio

def centro_de_masa(agente):
    # Variables para coordenadas de centro de masa
    x_cm = 0
    y_cm = 0
    
    # Variable para guardar la masa total del sistema
    M = 0
    
    for agente in agentes:
        
        # Calculo el peso de cada posicion
        x_cm += agente.masa * agente.pos[0]
        y_cm += agente.masa * agente.pos[1]
        M += agente.masa
    
    # Calculo coordenadas del centro de masa
    r_cm = [ x_cm/M, y_cm/M ]
    
    return r_cm
    

def radius_desviacion_estandar(agentes):
    # Variable para la sumatoria del radio menos la media al cuadrado
    sum = 0
    
    # Calculo la media
    media = get_media_radio(agentes)
    
    for agent in agentes:
        
        # Calculo el radio 
        r  = math.hypot(agent.pos[0], agent.pos[1])
        
        # Agrego nuevo valor a la sumatoria
        sum += (r - media)**2
    
    # Calculo la varianza
    sigma_cuad = sum / len(agentes)
    
    # Calculo la desviacion estandar
    desviacion_estandar = math.sqrt(sigma_cuad)
        
    # Devuelvo la desviacion estandar
    return desviacion_estandar 
    

# Desc: Calcula la media de la energia total del sistema
# Ret: Devuelve la media de la energia total del sistema
def kinetic_average():
    global agentes
    # Variable con la energia total del sistema
    k = total_kinetic()
    
    # Devuelve la media de la K total
    return k/len(agentes)

# Desc: Calcula la media de una lista de valores
# Ret: Devuelve la media de la lista que se le pasa
def average(values):
    # Devuelvo la media de la lista 'values'
    return math.fsum(values) / len(values)

def load_gradient_constants(config):
    
    # INTRODUCCION DE DATOS PARA CALCULO GRADIENTE
    GLOBAL_VAR['C_a'] = config.get_amplitud_atractiva()
    GLOBAL_VAR['C_r'] = config.get_amplitud_repulsiva()
    GLOBAL_VAR['l_a']= config.get_rango_de_atraccion()
    GLOBAL_VAR['l_r']= config.get_rango_de_repulsion()
    GLOBAL_VAR['alpha'] = config.get_alpha()
    GLOBAL_VAR['beta'] = config.get_beta()    


def load_vis_constants(config):
    global VISILIBITY 
    
    # CARGA DE DATOS DE SENSOR DE VISION
    VISILIBITY ['range'] = config.get_vis_range()
    VISILIBITY ['amplitude'] = config.get_vis_amplitude()
    VISILIBITY ['resolution'] = config.get_vis_resolution()
    VISILIBITY ['epsilon'] = config.get_vis_epsilon()
    VISILIBITY ['visible_cone'] = config.get_vis_is_visible_cone()
    VISILIBITY ['enable'] = config.get_vis_is_enable()    
    
def check_change(actual_value, last_value, min_diff, counter):
    
    # Calculo el cambio en la K total (dk)
    diff_value = math.fabs(actual_value - last_value)
            
    # Pregunto si dk es suficientemente chico
    if diff_value < min_diff:

        # Aumento contador de estabilidad de la K total
        counter += 1

    elif diff_value > 1:

        # Reseteo contador de diferencia chica en K
        counter = 0
            
    return actual_value, counter


def check_stability(x, y, slope, counter):
    # Construyo una recta con los valores que se pasan y calculo su pendiente.
    # Si la pendiente es menor a la seteada en el .xml entonces aumenta el
    # contador de estabilidad. Una vez que el contador alcanze cierto valor
    # se para la simulacion.
    # Ver que hacer si la pendiente se mantiene un tiempo menor al valor
    # deseado y luego tiene un pico repentino.
    # Posibilidades:
    #   * contar los picos y relajar la condicion de la pendiente
    
    # Construyo la recta para los valores x e y
    p = numpy.polyfit(x, y, 1)
    
    # Obtengo la pendiente
    m = p[0]
    
    
    if m < slope :
        
        # Si la pendiente es menor a la deseada, aumento el contador
        counter += 1
        
    elif counter > 2:
        
        # Si la pendiente no es menor a la deseada, pero ya se obtuvo una 
        # pendiente menor a la deseada algunas veces, entonces solo decremento
        # un poco el contador. 
        counter -= 2
        
    else:

        # Caso contrario, reseteo el contador.
        counter = 0
        
    return counter
    
def distance(agent1, agent2):
    x = agent1.pos[0] - agent2.pos[0]
    y = agent1.pos[1] - agent2.pos[1]
    
    dist = math.hypot(x,y)
    
    return dist




# Desc: Funcion que dice si 'dist0' es menor a 'dist1', False caso contrario
# Ret: (Bool) True sii 'dist0' <= 'dist1'
# Nota: -1 se usa como numero maximo.
def menor (dist0, dist1):
    # Chequea si 'dist0' es 'INFINITO'
    if dist0 == -1:
        return False
    
    # Chequea si 'dist1' es 'INFINITO'
    if dist1 == -1:
        return True
    
    # Chequea si 'dist0' <= 'dist1'
    return  d0 <= d1

def cellGradient(cells):
    ''' Dada un array de numpy calcula la direfencia entre la primera columna
        y la ultima y entre la primera fila y la ultima'''
    if cells.shape[0] == 0 or cells.shape[1] == 0:
        return numpy.zeros(2)
            
    pud = numpy.sum(abs(cells[[0,-1],:]),1)/cells.shape[0]
    plr = numpy.sum(abs(cells[:,[0,-1]]),0)/cells.shape[1]
  #  print "presion aariba/abajo ", pud, "presion izq/der", plr
    return numpy.hstack((numpy.diff(plr), numpy.diff(pud)))

